/*
 * findCorners.cpp
 *
 *  Created on: Feb 7, 2012
 *      Author: frc3928
 */
#include "findCorners.h"
#define DEBUG

int getAxis1Score(CvPoint2D32f pt, int imgWidth) //returns a score for BL-TR axis
{
	return pt.y*(imgWidth-pt.x);
}

int getAxis2Score(CvPoint2D32f pt) //returns a score for TL-BR axis
{
	return pt.x*pt.y;
}

/**
 * Finds corners of a contour representing a vision target
 * @param pointSeq a sequence of points in the contour.
 */
void findIntersections(CvSeq* pointSeq, CvPoint2D32f *arrayToStorePointsIn, int *numberOfPoints)
{
	short numPoints = pointSeq->total;
	CvPoint pointArray[numPoints];
	cvCvtSeqToArray(pointSeq, pointArray, CV_WHOLE_SEQ);

	//	for (int i=0; i<numPoints; i++)
	//	{
	//		std::cout << "Point " << i <<": ("<< pointArray[i].x << "," << pointArray[i].y << ")\n";
	//	}

	Line::Line lines[numPoints];

	for (int i = 0; i<(numPoints); i++)
	{
		if (i>0)
		{
			lines[i] = Line::Line(pointArray[i], pointArray[i-1]);
		}
		else
		{
			lines[i] = Line::Line(pointArray[i], pointArray[numPoints-1]);
		}


	}//finish creating lines

	//std::cout << "Finished creating lines.\n";

	//create an array of points to store the intersections of the lines.

	//go through and come up with intersections of every line
	for (int i = 0; i<numPoints; i++)
	{
		for (int j = 0; j<numPoints; j++)
		{
			if (i != j)
			{
				int arrayNumber = 0;
				arrayNumber = (i*numPoints + j) + (*numberOfPoints);
				arrayToStorePointsIn[arrayNumber] = lines[i].getIntersectionWith(lines[j]);
				//std::cout << "Made intersection [" << arrayNumber << "]: (" << arrayToStorePointsIn[arrayNumber].x << ", " << arrayToStorePointsIn[arrayNumber].y << ")\n";
			}
		}

	}
	*numberOfPoints = (numPoints*numPoints) + *numberOfPoints;

}

void filterIntersections(CvPoint2D32f *arrayToStorePointsIn, int *numberOfPoints, CvRect boundingBox, int boundingTolerance, CvPoint2D32f *cornerSet, int imageWidth)
{
	int minX = boundingBox.x - boundingTolerance;
	int minY = boundingBox.y - boundingTolerance;
	int maxX = boundingBox.x + boundingBox.width + boundingTolerance;
	int maxY = boundingBox.y + boundingBox.height + boundingTolerance;
	//std::cout << "Searching for points in a bounding box of (" << minX << "," << minY <<") to (" << maxX << "," << maxY << "). \n";

	CvPoint2D32f currentPoint;

	int axis1Score = 0;
	int axis2Score = 0;


	unsigned int axis1Min = -1;
	unsigned int axis1Max = 0;
	unsigned int axis2Min = -1;
	unsigned int axis2Max = 0;

	int topRightBestIndex = 0;
	int topLeftBestIndex = 0;
	int bottomRightBestIndex = 0;
	int bottomLeftBestIndex = 0;



	for (int i=0; i<*numberOfPoints; i++) //cull points outside bounding box
	{
		currentPoint = arrayToStorePointsIn[i];

		axis1Score = getAxis1Score(currentPoint, imageWidth);
		axis2Score = getAxis2Score(currentPoint);

		if (currentPoint.x > minX &&
				currentPoint.x < maxX &&
				currentPoint.y > minY &&
				currentPoint.y < maxY &&
				*numberOfPoints > 3)
		{
			//Select corner points
			//select for top left
			if (axis2Score < axis2Min)
			{
				axis2Min = axis2Score;
				topLeftBestIndex = i;
			}

			//select for top right
			if (axis1Score < axis1Min)
			{
				axis1Min = axis1Score;
				topRightBestIndex = i;
			}

			//select for bottom left
			if (axis1Score > axis1Max)
			{
				axis1Max = axis1Score;
				bottomLeftBestIndex = i;
			}

			//select for bottom right
			if (axis2Score > axis2Max)
			{
				axis2Max = axis2Score;
				bottomRightBestIndex = i;
			}

		}
		else
		{
			//culling
			arrayToStorePointsIn[i] = cvPoint2D32f(-1, -1);
		}
	}

	//pass out the corner array.
	cornerSet[0] = arrayToStorePointsIn[topLeftBestIndex];
	cornerSet[1] = arrayToStorePointsIn[topRightBestIndex];
	cornerSet[2] = arrayToStorePointsIn[bottomLeftBestIndex];
	cornerSet[3] = arrayToStorePointsIn[bottomRightBestIndex];

	if (*numberOfPoints <4)
	{
		for (int i=0; i<4; i++)
		{
			cornerSet[i]= cvPoint2D32f(-1, -1);
		}
	}


}

void findMinimumAreaKGon(CvPoint2D32f points, int k)
{

}
